#include "common_tool/csv_parser.h"
#include "common_tool/csv_writer.h"
#include <yaml-cpp/yaml.h>
#include "rtk_data_path.h"
#include "string"
#include "cmath"
#include "matplotlibcpp.hpp"
namespace plt = matplotlibcpp;
int main(int argc, char **argv)
{
	std::string config_file = rtk_config_path + "u_shape.yaml";
	YAML::Node config_yaml = YAML::LoadFile(config_file); //也可以这样读取文件
	int size = config_yaml["u_shapes"].size();
	std::string filename_prefix = rtk_data_path + "u_shape";
	for (int i = 0; i < size; i++)
	{
		double straight_line_length = config_yaml["u_shapes"][i]["straight_line_length"].as<double>();
		double arc_length = config_yaml["u_shapes"][i]["arc_length"].as<double>();
		double radius = arc_length / M_PI;
		double speed = config_yaml["u_shapes"][i]["speed"].as<double>();
		double delta_s = config_yaml["u_shapes"][i]["delta_s"].as<double>();
		std::vector<double> x_vector;
		std::vector<double> y_vector;
		std::vector<double> theta_vector;
		std::vector<double> kappa_vector;
		std::vector<double> v_vector;
		std::vector<double> s_vector;
		std::vector<double> a_vector;
		double sum_s = 0;
		//1st segement straght_line
		for (int i = 1; i * delta_s <= straight_line_length; i++)
		{
			x_vector.push_back(-straight_line_length / 2.0 + i * delta_s);
			y_vector.push_back(0);
			theta_vector.push_back(0);
			kappa_vector.push_back(0);
			v_vector.push_back(speed);
			sum_s += delta_s;
			s_vector.push_back(sum_s);
			a_vector.push_back(0);
		}
		//2nd segment half circle
		double center_x = straight_line_length / 2.0;
		double center_y = radius;
		for (int i = 1; i * delta_s <= arc_length; i++)
		{
			double temp_theta = i * delta_s / radius;
			x_vector.push_back(center_x + radius * std::cos(-M_PI / 2.0 + temp_theta));
			y_vector.push_back(center_y + radius * std::sin(-M_PI / 2.0 + temp_theta));
			theta_vector.push_back(temp_theta);
			kappa_vector.push_back(1.0 / radius);
			v_vector.push_back(speed);
			sum_s += delta_s;
			s_vector.push_back(sum_s);
			a_vector.push_back(0);
		}
		//3rd segment line
		for (int i = 1; i * delta_s <= straight_line_length; i++)
		{
			x_vector.push_back(straight_line_length / 2.0 - i * delta_s);
			y_vector.push_back(2 * radius);
			theta_vector.push_back(M_PI);
			kappa_vector.push_back(0);
			v_vector.push_back(speed);
			sum_s += delta_s;
			s_vector.push_back(sum_s);
			a_vector.push_back(0);
		}
		//4th segement circle
		center_x = -straight_line_length / 2.0;
		center_y = radius;
		for (int i = 1; i * delta_s <= arc_length; i++)
		{
			double temp_theta = M_PI + i * delta_s / radius;
			x_vector.push_back(center_x + radius * std::cos(-M_PI / 2.0 + temp_theta));
			y_vector.push_back(center_y + radius * std::sin(-M_PI / 2.0 + temp_theta));
			theta_vector.push_back(temp_theta);
			kappa_vector.push_back(1.0 / radius);
			v_vector.push_back(speed);
			sum_s += delta_s;
			s_vector.push_back(sum_s);
			a_vector.push_back(0);
		}
		//
		double pre_yaw_deg = config_yaml["u_shapes"][i]["pre_yaw_deg"].as<double>();
		double pre_yaw_rad = config_yaml["u_shapes"][i]["pre_yaw_deg"].as<double>() / 180.0 * M_PI;
		for (int i = 0; i < x_vector.size(); i++)
		{
			double temp_x = x_vector[i] * std::cos(pre_yaw_rad) + y_vector[i] * std::sin(pre_yaw_rad);
			y_vector[i] = -x_vector[i] * std::sin(pre_yaw_rad) + y_vector[i] * std::cos(pre_yaw_rad);
			x_vector[i] = temp_x;
			theta_vector[i] = theta_vector[i] + pre_yaw_rad;
		}
		std::cout << "x size:" << x_vector.size() << std::endl;
		std::cout << "y size:" << y_vector.size() << std::endl;
		std::cout << "theta size:" << theta_vector.size() << std::endl;

		std::string filename = filename_prefix + "L" + std::to_string(int(straight_line_length)) + "R" +
							   std::to_string(int(radius)) + ".csv";
		io::CSVWriter rtk_csv(filename);
		std::vector<std::string> head_string = {"x", "y", "theta", "kappa", "v", "s", "a"};
		rtk_csv.write_row<std::string>(head_string);
		for (int i = 0; i < x_vector.size(); i++)
		{
			std::vector<double> temp_vector;
			temp_vector.push_back(x_vector[i]);
			temp_vector.push_back(y_vector[i]);
			temp_vector.push_back(theta_vector[i] * 180 / M_PI);
			temp_vector.push_back(kappa_vector[i]);
			temp_vector.push_back(v_vector[i]);
			temp_vector.push_back(s_vector[i]);
			temp_vector.push_back(a_vector[i]);
			rtk_csv.write_row<double>(temp_vector);
		}
		plt::plot(x_vector, y_vector);
		plt::show();
	}
}